#include "TurtleSimDialog.h"

//(*InternalHeaders(TurtleSimDialog)
#include <wx/intl.h>
#include <wx/string.h>
//*)


//(*IdInit(TurtleSimDialog)
const long TurtleSimDialog::ID_STATICTEXT1 = wxNewId();
const long TurtleSimDialog::ID_STATICTEXT2 = wxNewId();
const long TurtleSimDialog::ID_STATICTEXT3 = wxNewId();
const long TurtleSimDialog::ID_TEXTCTRL1 = wxNewId();
const long TurtleSimDialog::ID_TEXTCTRL2 = wxNewId();
const long TurtleSimDialog::ID_TEXTCTRL3 = wxNewId();
const long TurtleSimDialog::ID_STATICTEXT4 = wxNewId();
const long TurtleSimDialog::ID_STATICTEXT5 = wxNewId();
const long TurtleSimDialog::ID_STATICTEXT6 = wxNewId();
const long TurtleSimDialog::ID_SPINCTRL1 = wxNewId();
const long TurtleSimDialog::ID_SPINCTRL2 = wxNewId();
const long TurtleSimDialog::ID_STATICTEXT7 = wxNewId();
const long TurtleSimDialog::ID_BUTTON1 = wxNewId();
const long TurtleSimDialog::ID_PANEL1 = wxNewId();
const long TurtleSimDialog::ID_TIMER1 = wxNewId();
//*)

BEGIN_EVENT_TABLE(TurtleSimDialog,wxDialog)
	//(*EventTable(TurtleSimDialog)
	//*)
END_EVENT_TABLE()



TurtleSimDialog::TurtleSimDialog(wxWindow* parent,wxWindowID id)
{
	//(*Initialize(TurtleSimDialog)
	wxGridSizer* GridSizer1;
	wxBoxSizer* BoxSizer1;

	Create(parent, wxID_ANY, wxEmptyString, wxDefaultPosition, wxDefaultSize, wxDEFAULT_DIALOG_STYLE, _T("wxID_ANY"));
	BoxSizer1 = new wxBoxSizer(wxHORIZONTAL);
	Panel1 = new wxPanel(this, ID_PANEL1, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL1"));
	GridSizer1 = new wxGridSizer(0, 3, 0, 0);
	StaticText1 = new wxStaticText(Panel1, ID_STATICTEXT1, _("X"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT1"));
	GridSizer1->Add(StaticText1, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	StaticText2 = new wxStaticText(Panel1, ID_STATICTEXT2, _("Y"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT2"));
	GridSizer1->Add(StaticText2, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	StaticText3 = new wxStaticText(Panel1, ID_STATICTEXT3, _("Theta"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT3"));
	GridSizer1->Add(StaticText3, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	TextCtrl_X = new wxTextCtrl(Panel1, ID_TEXTCTRL1, _("0"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_TEXTCTRL1"));
	TextCtrl_X->Disable();
	GridSizer1->Add(TextCtrl_X, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	TextCtrl_Y = new wxTextCtrl(Panel1, ID_TEXTCTRL2, _("0,00"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_TEXTCTRL2"));
	TextCtrl_Y->Disable();
	GridSizer1->Add(TextCtrl_Y, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	TextCtrl_Theta = new wxTextCtrl(Panel1, ID_TEXTCTRL3, _("0.00"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_TEXTCTRL3"));
	TextCtrl_Theta->Disable();
	GridSizer1->Add(TextCtrl_Theta, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	StaticText4 = new wxStaticText(Panel1, ID_STATICTEXT4, _("Linear speed"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT4"));
	GridSizer1->Add(StaticText4, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	StaticText5 = new wxStaticText(Panel1, ID_STATICTEXT5, _("Angular speed"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT5"));
	GridSizer1->Add(StaticText5, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	StaticText6 = new wxStaticText(Panel1, ID_STATICTEXT6, wxEmptyString, wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT6"));
	GridSizer1->Add(StaticText6, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	SpinCtrlLinearSpeed = new wxSpinCtrl(Panel1, ID_SPINCTRL1, _T("0"), wxDefaultPosition, wxDefaultSize, 0, -5, 5, 0, _T("ID_SPINCTRL1"));
	SpinCtrlLinearSpeed->SetValue(_T("0"));
	GridSizer1->Add(SpinCtrlLinearSpeed, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	SpinCtrlAngularSpeed = new wxSpinCtrl(Panel1, ID_SPINCTRL2, _T("0"), wxDefaultPosition, wxDefaultSize, 0, -5, 5, 0, _T("ID_SPINCTRL2"));
	SpinCtrlAngularSpeed->SetValue(_T("0"));
	GridSizer1->Add(SpinCtrlAngularSpeed, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	StaticText7 = new wxStaticText(Panel1, ID_STATICTEXT7, wxEmptyString, wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT7"));
	GridSizer1->Add(StaticText7, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	ButtonClear = new wxButton(Panel1, ID_BUTTON1, _("Clear"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON1"));
	GridSizer1->Add(ButtonClear, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	Panel1->SetSizer(GridSizer1);
	GridSizer1->Fit(Panel1);
	GridSizer1->SetSizeHints(Panel1);
	BoxSizer1->Add(Panel1, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	SetSizer(BoxSizer1);
	Timer1.SetOwner(this, ID_TIMER1);
	Timer1.Start(1000, false);
	BoxSizer1->Fit(this);
	BoxSizer1->SetSizeHints(this);

	Connect(ID_SPINCTRL1,wxEVT_COMMAND_SPINCTRL_UPDATED,(wxObjectEventFunction)&TurtleSimDialog::OnSpinCtrlLinearSpeedChange);
	Connect(ID_SPINCTRL2,wxEVT_COMMAND_SPINCTRL_UPDATED,(wxObjectEventFunction)&TurtleSimDialog::OnSpinCtrlAngularSpeedChange);
	Connect(ID_BUTTON1,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&TurtleSimDialog::OnButtonClearClick);
	Connect(ID_TIMER1,wxEVT_TIMER,(wxObjectEventFunction)&TurtleSimDialog::OnTimer1Trigger);
	//*)
    //ros::NodeHandle n;
    //ros::init(argc, argv, "listener");
   // printf("Dialog: suscribing to ros\n");
    //ros::Subscriber sub = n.subscribe("/turtle1/pose", 100, &TurtleSimDialog::turttleposeCallback, this);
// %EndTag(SUBSCRIBER)%

  /**
   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
   * callbacks will be called from within this thread (the main one).  ros::spin()
   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
   */
// %Tag(SPIN)%
//  ros::spin();
    //ROS_INFO("as");
}

TurtleSimDialog::~TurtleSimDialog()
{
	//(*Destroy(TurtleSimDialog)
	//*)
}

void TurtleSimDialog::turttleposeCallback(const turtlesim::Pose::ConstPtr& msg)
{
    wxString wxS;// cX[20] = "\0", cY[20] = "\0", cT[20] = "\0";
    turtlePose.x = msg->x;
    turtlePose.y = msg->y;
    turtlePose.theta = msg->theta;
    turtlePose.linear_velocity = msg->linear_velocity;
    turtlePose.angular_velocity = msg->angular_velocity;
    //sprintf(cX,"%.2lf", turtlePose.x);sprintf(cY,"%.2lf", turtlePose.y);sprintf(cT,"%.2lf", turtlePose.theta);
//    wxString s("df");
    wxS = wxString::Format(_T("%.2lf"), turtlePose.x);
    TextCtrl_X->ChangeValue( wxS);
    TextCtrl_X->SendTextUpdatedEvent();
    wxS = wxString::Format(_T("%.2lf"), turtlePose.y);
    TextCtrl_Y->ChangeValue( wxS );
    wxS = wxString::Format(_T("%.2lf"), turtlePose.theta);
    TextCtrl_Theta->ChangeValue( wxS );
    //ROS_INFO("I heard: [x, y, theta] = [%f, %f, %f] [v = %f, w = %f]\n", msg->x, msg->y, msg->theta, msg->linear_velocity, msg->angular_velocity);

}

void TurtleSimDialog::OnPanel1Paint(wxPaintEvent& event)
{

  // std::cout << "Painting" << std::endl;

}

void TurtleSimDialog::OnSpinCtrlLinearSpeedChange(wxSpinEvent& event)
{
    std::cout << "Linear vel = " << event.GetPosition() << std::endl;
    if(event.GetPosition() > 0)
        turtleVelocity.linear = 1;
    else
        turtleVelocity.linear = -1;
    turtleVelocity.angular = 0.0;
    pub.publish(turtleVelocity);
}

void TurtleSimDialog::OnSpinCtrlAngularSpeedChange(wxSpinEvent& event)
{
    std::cout << "Angular vel = " << event.GetPosition() << std::endl;
    if(event.GetPosition() > 0)
        turtleVelocity.angular = 1;
    else
        turtleVelocity.angular = -1;
    turtleVelocity.linear = 0.0;
    pub.publish(turtleVelocity);
}

void TurtleSimDialog::OnTimer1Trigger(wxTimerEvent& event)
{
    this->Refresh();
}

void TurtleSimDialog::OnButtonClearClick(wxCommandEvent& event)
{
    if (client_clear.call(srv_clear)){
        ROS_INFO("Clear sent");
    }else{
        ROS_ERROR("Error sending clear");
    }
}
